#include "arm_control/arm_action_type4.hpp"
#include <chrono>
#include <thread>

//转弯动作

namespace arm_control
{

ArmActionType4::ArmActionType4(rclcpp::Logger logger, rclcpp::Node::SharedPtr node)
  : ArmActionBase(logger), node_(node)
{
  // 初始化MoveGroupInterface，分别为左右机械臂
  move_group_left_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_left");
  move_group_right_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_right");
  
  // 初始化爪子发布器
  zhuazi_left_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_left", 10);
  zhuazi_right_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_right", 10);
  
  RCLCPP_INFO(logger_, "动作类型4初始化完成");
  RCLCPP_INFO(logger_, "MoveIt接口已初始化，Planning Group: arm_left 和 arm_right");
  RCLCPP_INFO(logger_, "爪子发布器已创建: /zhuazi_left 和 /zhuazi_right");
}

void ArmActionType4::execute(
  const std::shared_ptr<GoalHandleARM> goal_handle,
  std::shared_ptr<ARM::Feedback> feedback,
  std::shared_ptr<ARM::Result> result)
{
  RCLCPP_INFO(logger_, "执行动作类型4 - 左臂移动后等待，再移动右臂");
  
  // ========== 初始化：将huakuai1和huakuai2移动到0.0 ==========
  
  // 1. 先将huakuai2（左臂）移动到0.0
  feedback->progress_percentage = 5.0;
  feedback->current_status = "初始化：huakuai2移动到0.0...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "设置huakuai2（左臂）目标位置为0.0");
  
  // 更新起始状态为当前状态
  move_group_left_->setStartStateToCurrentState();
  
  // 获取当前所有关节值
  std::vector<double> left_joint_values = move_group_left_->getCurrentJointValues();
  
  // 只修改huakuai2的值
  const std::vector<std::string>& left_joint_names = move_group_left_->getJointNames();
  for (size_t i = 0; i < left_joint_names.size(); ++i) {
    if (left_joint_names[i] == "huakuai2") {
      left_joint_values[i] = 0.0;
      RCLCPP_INFO(logger_, "找到huakuai2关节，索引: %zu, 设置目标值为0.0", i);
      break;
    }
  }
  
  // 设置目标关节值
  move_group_left_->setJointValueTarget(left_joint_values);
  
  moveit::planning_interface::MoveGroupInterface::Plan huakuai2_plan;
  bool huakuai2_success = (move_group_left_->plan(huakuai2_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!huakuai2_success) {
    result->success = false;
    result->message = "动作类型4失败 - huakuai2初始化路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "huakuai2初始化路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "huakuai2初始化路径规划成功");
  
  auto huakuai2_execute_result = move_group_left_->execute(huakuai2_plan);
  if (huakuai2_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型4失败 - huakuai2初始化运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "huakuai2初始化运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "huakuai2已到达0.0位置");
  
  // 2. 将huakuai1（右臂）移动到0.0
  feedback->progress_percentage = 8.0;
  feedback->current_status = "初始化：huakuai1移动到0.0...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "设置huakuai1（右臂）目标位置为0.0");
  
  // 更新起始状态为当前状态
  move_group_right_->setStartStateToCurrentState();
  
  // 获取当前所有关节值
  std::vector<double> right_joint_values = move_group_right_->getCurrentJointValues();
  
  // 只修改huakuai1的值
  const std::vector<std::string>& right_joint_names = move_group_right_->getJointNames();
  for (size_t i = 0; i < right_joint_names.size(); ++i) {
    if (right_joint_names[i] == "huakuai1") {
      right_joint_values[i] = 0.0;
      RCLCPP_INFO(logger_, "找到huakuai1关节，索引: %zu, 设置目标值为0.0", i);
      break;
    }
  }
  
  // 设置目标关节值
  move_group_right_->setJointValueTarget(right_joint_values);
  
  moveit::planning_interface::MoveGroupInterface::Plan huakuai1_plan;
  bool huakuai1_success = (move_group_right_->plan(huakuai1_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!huakuai1_success) {
    result->success = false;
    result->message = "动作类型4失败 - huakuai1初始化路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "huakuai1初始化路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "huakuai1初始化路径规划成功");
  
  auto huakuai1_execute_result = move_group_right_->execute(huakuai1_plan);
  if (huakuai1_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型4失败 - huakuai1初始化运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "huakuai1初始化运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "huakuai1已到达0.0位置");
  RCLCPP_INFO(logger_, "初始化完成，开始执行主要动作");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型4已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型4已取消");
    return;
  }
  
  // ========== 左臂操作流程 ==========
  
  // 1. 左臂运动到A_turn_left_pose
  feedback->progress_percentage = 10.0;
  feedback->current_status = "左臂移动到A_turn_left_pose...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "设置左臂目标姿态为A_turn_left_pose");
  
  move_group_left_->setStartStateToCurrentState();
  move_group_left_->setNamedTarget("A_turn_left_pose");
  
  moveit::planning_interface::MoveGroupInterface::Plan left_plan;
  bool left_success = (move_group_left_->plan(left_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!left_success) {
    result->success = false;
    result->message = "动作类型4失败 - 左臂到A_turn_left_pose路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂到A_turn_left_pose路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂到A_turn_left_pose路径规划成功");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型4已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型4已取消");
    return;
  }
  
  feedback->progress_percentage = 30.0;
  feedback->current_status = "左臂正在移动...";
  goal_handle->publish_feedback(feedback);
  
  auto left_execute_result = move_group_left_->execute(left_plan);
  if (left_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型4失败 - 左臂到A_turn_left_pose运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂到A_turn_left_pose运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂已到达A_turn_left_pose");
  
  // 发布/zhuazi_left为0
  std_msgs::msg::Int32 left_msg;
  left_msg.data = 0;
  zhuazi_left_publisher_->publish(left_msg);
  RCLCPP_INFO(logger_, "发布到 /zhuazi_left: 0");
  
  // 2. 等待1秒
  feedback->progress_percentage = 50.0;
  feedback->current_status = "等待1秒...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "左臂到位，等待1秒...");
  std::this_thread::sleep_for(std::chrono::seconds(1));
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型4已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型4已取消");
    return;
  }
  
  // ========== 右臂操作流程 ==========
  
  // 先更新机器人状态（包括左臂执行后的所有关节状态）
  RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态...");
  move_group_right_->setStartStateToCurrentState();
  
  // 3. 右臂运动到A_turn_right_pose
  feedback->progress_percentage = 60.0;
  feedback->current_status = "右臂移动到A_turn_right_pose...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "设置右臂目标姿态为A_turn_right_pose");
  
  move_group_right_->setStartStateToCurrentState();
  move_group_right_->setNamedTarget("A_turn_right_pose");
  
  moveit::planning_interface::MoveGroupInterface::Plan right_plan;
  bool right_success = (move_group_right_->plan(right_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!right_success) {
    result->success = false;
    result->message = "动作类型4失败 - 右臂到A_turn_right_pose路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂到A_turn_right_pose路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂到A_turn_right_pose路径规划成功");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型4已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型4已取消");
    return;
  }
  
  feedback->progress_percentage = 80.0;
  feedback->current_status = "右臂正在移动...";
  goal_handle->publish_feedback(feedback);
  
  auto right_execute_result = move_group_right_->execute(right_plan);
  if (right_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型4失败 - 右臂到A_turn_right_pose运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂到A_turn_right_pose运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂已到达A_turn_right_pose");
  
  // 发布/zhuazi_right为0
  std_msgs::msg::Int32 right_msg;
  right_msg.data = 0;
  zhuazi_right_publisher_->publish(right_msg);
  RCLCPP_INFO(logger_, "发布到 /zhuazi_right: 0");
  
  // 动作完成
  feedback->progress_percentage = 100.0;
  feedback->current_status = "动作类型4任务完成";
  goal_handle->publish_feedback(feedback);
  
  result->success = true;
  result->message = "动作类型4执行成功 - 左臂和右臂已完成转向动作";
  goal_handle->succeed(result);
  RCLCPP_INFO(logger_, "动作类型4执行成功 - 左臂和右臂已完成转向动作");
}

} // namespace arm_control
